/*
 * @Author: your name
 * @Date: 2022-01-06 16:55:52
 * @LastEditTime: 2022-04-19 16:05:45
 * @LastEditors: Please set LastEditors
 * @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 * @FilePath: \025 SDF_FR8012H_DJBC\examples\none_evm\ble_simple_peripheral\sd_code\sd_app_007DJBC.c
 */
/**
  ******************************************************************************
  * @file    xxx.c 
  * @author  SD Electronics xxx
  * @version V1.0
  * @date    xx-xx-xx
  * @brief   xxxxxxxxxxxxxxxxxx.
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "sd_common.h"
#include "sh3001_attitude.h"
#include "user_task.h"
#include "math.h"
#define SAMPLE_DEEP 5
os_timer_t OStimer_DJBC;
extern uint16_t vbat_vol;
/** 
* @brief  校准数据
  */
short cal_accX = -1360;
short cal_accY = 430;
short cal_accZ = 2448;

short cal_gyrX = 454;
short cal_gyrY = -350;
short cal_gyrZ = -14;

long sample_acc[3] = {0};
long sample_gyr[3] = {0};
short test_acc[3];
short test_gyr[3];
float real_acc[3];
float test_euler_angle[3];
/** 
  * @brief  设备管理结构数据初始化定义
  */ 
ST_DJBC_PROCO st_DJBC_Proco;
/**
  * @}
  */
uint16_t BABY_FRQ_TASK_ID;
void baby_freq_task_event(void);

/**
  * @brief  xxx
  * @param  None
  * @note	xxx
  * @retval None
  */
static void DJBC_Run(void *param)
{
    //co_printf("usr: DJBC RUN.\r\n");
    #if 1
    uint8_t DJBC_Data[16];
    static uint32_t data_send_count = 0;
    static uint8_t baby_freq_count = 0;
   
    /* NTC */
    PST_DEV_MANAGE pstNTC_DevManage = SDDevManageCtrl(EN_DEV_NTC, EN_READ_NTC_VOL, 0, NULL);
    if(pstNTC_DevManage != NULL)
    {
        pstNTC_DevManage = SDDevManageCtrl(EN_DEV_NTC, EN_READ_NTC_TEMP, 0, NULL);
        if(pstNTC_DevManage != NULL)
        {
            PST_NTC_CTRL pstNTCCtrl = (PST_NTC_CTRL)pstNTC_DevManage->pvData;
            /* 赋值温度数据 */
            st_DJBC_Proco.Temp = pstNTCCtrl->pstNTCInfo->NTC_Temp;
            #if 1
            PST_DEV_MANAGE pstSH3001_DevManage = SDDevManageRead(EN_DEV_SH3001, 0, NULL, NULL);
            if(pstSH3001_DevManage != NULL)
            {
                //accData[3]: acc X,Y,Z;  gyroData[3]: gyro X,Y,Z;
                PST_SH3001_CTRL pstSH3001Ctrl = (PST_SH3001_CTRL)pstSH3001_DevManage->pvData;

                sample_acc[0] = sample_acc[0] + pstSH3001Ctrl->pstSH3001Info->accData[0];
                sample_acc[1] = sample_acc[1] + pstSH3001Ctrl->pstSH3001Info->accData[1];
                sample_acc[2] = sample_acc[2] + pstSH3001Ctrl->pstSH3001Info->accData[2];

                sample_gyr[0] = sample_gyr[0] + pstSH3001Ctrl->pstSH3001Info->gyroData[0];
                sample_gyr[1] = sample_gyr[1] + pstSH3001Ctrl->pstSH3001Info->gyroData[1];
                sample_gyr[2] = sample_gyr[2] + pstSH3001Ctrl->pstSH3001Info->gyroData[2];

                //if(sample_count ++ == SAMPLE_DEEP)
                {
                    sample_acc[0] = sample_acc[0]/SAMPLE_DEEP - cal_accX;
                    sample_acc[1] = sample_acc[1]/SAMPLE_DEEP - cal_accY;
                    sample_acc[2] = sample_acc[2]/SAMPLE_DEEP + 2048 - cal_accZ;

                    sample_gyr[0] = sample_gyr[0]/SAMPLE_DEEP - cal_gyrX;
                    sample_gyr[1] = sample_gyr[1]/SAMPLE_DEEP - cal_gyrY;
                    sample_gyr[2] = sample_gyr[2]/SAMPLE_DEEP - cal_gyrZ;

                    /* 陀螺仪数据计算并且赋值到协议结构体中去 */
                    #if 1
                    //co_printf("cycle:%d  %d  %d  %d  %d  %d.\r\n", 
                                    //sample_acc[0], sample_acc[1], sample_acc[2],
                                    //sample_gyr[0],  sample_gyr[1],  sample_gyr[2]);
                    
                    test_acc[0] = sample_acc[0];
                    test_acc[1] = sample_acc[1];
                    test_acc[2] = sample_acc[2];

                    test_gyr[0] = sample_gyr[0];
                    test_gyr[1] = sample_gyr[1];
                    test_gyr[2] = sample_gyr[2];

                    #if 0
                    co_printf("test :%d  %d  %d  %d  %d  %d.\r\n", 
                                    test_acc[0], test_acc[1], test_acc[2],
                                    test_gyr[0],  test_gyr[1],  test_gyr[2]);
                    #endif
                    sh3001_run_attitude(test_acc, test_gyr, test_euler_angle);
                    //co_printf("test_euler_angle[1]:%0.1f\r\n", test_euler_angle[1]);
                     
                    /* 计算呼吸频率 */
                    if(baby_freq_count ++ == 4)
                    {
                        baby_freq_count = 0;
                        baby_freq_task_event();
                    }
                    /* 判断位置 */
                    if(test_euler_angle[0] >= -40.6 && test_euler_angle[0] <= -37.6)
                        st_DJBC_Proco.BabyPOS = EN_BABYPOS_UP;
                    else if(test_euler_angle[0] >= -49.6 && test_euler_angle[0] <= -46.6)
                        st_DJBC_Proco.BabyPOS = EN_BABYPOS_DOWN;
                    else
                        st_DJBC_Proco.BabyPOS = EN_BABYPOS_SIDE;

                    if(test_euler_angle[1] >= -163.6 && test_euler_angle[1] <= -154.6 && st_DJBC_Proco.BabyPOS != EN_BABYPOS_DOWN)
                        st_DJBC_Proco.BabyPOS = EN_BABYPOS_SIDE;
                    //printf("return %d.\r\n", attitude_value);
                    //printf("angle: %0.1f  %0.1f  %0.1f.\r\n", test_euler_angle[0], test_euler_angle[1], test_euler_angle[2]);
                    //printf("angle_test: %0.1f.\r\n",2.22);
                    
                    #endif
                }
                #if 1
                /* 1s发送一次数据 */
                if(data_send_count ++ == 2)
                {
                    data_send_count = 0;
                    sprintf((char *)DJBC_Data,"S:%d,F:%d,T:%0.1f,V:%d", st_DJBC_Proco.BabyPOS, st_DJBC_Proco.BreatheFreq, st_DJBC_Proco.Temp, vbat_vol);
                    extern uint8_t ntf_char1_enable;
                    if(ntf_char1_enable == true)
                        ntf_data(DJBC_Data, strlen((char *)DJBC_Data));
                    //usr_set_adv_data(DJBC_Data, sizeof(DJBC_Data));
                    //co_printf("%s.\r\n", DJBC_Data);
                }
                #endif
                
            }
            #else
           
            #endif
        }
    }
    #endif
}

/**
  * @brief  baby_freq_task_func
  * @param  None
  * @note	xxx
  * @retval None
  */
static int baby_freq_task_func(os_event_t *param)
{
    switch(param->event_id)
    {
        case USER_EVT_HAND_FREQ:
        {
            //co_printf("usr: baby freq handle......\r\n");
            #if 1
            static float z_acc = 0;
            static float z_acc_temp = 0;
			static float z_acc_sum = 0;
            static float baby_freq = 0;
           
            static uint8_t run_time = 0;

            get_acc_data(real_acc);
            z_acc = fabs(real_acc[1])*1000;
            z_acc_sum = z_acc_sum + z_acc;
			
            z_acc_temp = fabs(z_acc_temp);
            //printf("z_acc:%0.1f\r\n", z_acc);
            //printf("z_acc_temp=%0.1f.\r\n", z_acc_temp);
            //printf("usr: dif=%0.1f.\r\n", (z_acc_temp - z_acc));
          
            //静止状态
            #endif
			
			if((z_acc_temp - z_acc) > 2 && z_acc_temp != 0)
			{
				//printf("acc int = %0.1f.", z_acc_temp - z_acc);
				baby_freq ++;
			}
          
			z_acc_temp = z_acc;

            if(run_time++ == 5)
            {
                static int i = 0;
                run_time = 0;
                co_printf("time out: %d.\r\n", i++);
                if(i == 30)
                {
                    i = 0;
                    printf("usr: baby freq = %0.1f.\r\n", baby_freq);
                    st_DJBC_Proco.BreatheFreq = baby_freq;
                    //co_printf("usr: breathFreq = %d.\r\n",  st_DJBC_Proco.BreatheFreq);
                    baby_freq = 0;
                }
            }
        }
        break;
    }

    return EVT_CONSUMED;
}

/**
  * @brief  baby_freq_task_event
  * @param  None
  * @note	xxx
  * @retval None
  */
void baby_freq_task_event(void)
{
    os_event_t baby_freq_task_event;
    baby_freq_task_event.event_id = USER_EVT_HAND_FREQ;
    baby_freq_task_event.src_task_id = BABY_FRQ_TASK_ID;
    baby_freq_task_event.param = NULL;
    baby_freq_task_event.param_len = 0;
    os_msg_post(BABY_FRQ_TASK_ID, &baby_freq_task_event);
    //co_printf("usr: post task.\r\n");
}



/**
  * @brief  xxx
  * @param  None
  * @note	xxx
  * @retval None
  */
void DJBC_Init(void)
{
    int init_value = 0;

    test_acc[0] = -1357;
    test_acc[1] =  430;
    test_acc[2] = 2448;

    test_gyr[0] = 453;
    test_gyr[1] = -350;
    test_gyr[2] = -15;

    init_value = sh3001_init_attitude(test_acc, test_gyr);
    printf("init value = %d.\r\n", init_value);
    os_timer_init(&OStimer_DJBC, DJBC_Run, NULL);
    co_delay_100us(1);
    BABY_FRQ_TASK_ID = os_task_create(baby_freq_task_func);
    if(init_value == 0)
    {
        co_printf("usr: init sh3001 attitude success.\r\n");
        os_timer_start(&OStimer_DJBC, 50, true);
    }
}

/******************* (C) COPYRIGHT 2011 ShenZhenShiding electronics **************************/

